#include "dmke.h"
#include "../../bsp/cli_cmd.h"
#include "../../bsp/io_config.h"

static int __local_assert(xp_motor_t *motor)
{
    if (motor->inited != 1 ||
        motor->private_data == NULL ||
        motor->ops == NULL)
    {
        println("motor not inited.");
        return -1;
    }
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    if (mb_dev->bus == NULL)
    {
        println("motor %s not mount in bus of modbus", mb_dev->name);
        return -2;
    }
    return 0;
}

#define LOCAL_ASSERT(motor)    \
    if (__local_assert(motor)) \
    return -1

static int dmke_init(xp_motor_t *motor)
{
    // 配置电机加速度、减速带、使能等
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct dmke_motor_attr *motor_attr = (struct dmke_motor_attr *)(mb_dev->device_attr);

    unsigned short addr = 0x0005;
    unsigned short data = (motor_attr->control_mode == 1) ? 0x0000 : 0x0001; // 配置控制模式 485为内部速度 IO为5V电压输入
    int ret = bus->swrite(mb_dev, addr, data);
    if (ret != 0) return -1;
    // 配置电机为速度模式
    aos_msleep(200);
    addr = 0x0025;
    data = (motor_attr->control_mode == 1) ? 0x0001 : 0x0000;
    ret = bus->swrite(mb_dev, addr, data);

    return ret;
}

static int dmke_speed_set(xp_motor_t *motor, motor_rpm_t rpm)
{
    LOCAL_ASSERT(motor);

    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct dmke_motor_attr *motor_attr = (struct dmke_motor_attr *)(mb_dev->device_attr);

    unsigned short speed_addr = 0x0006;
    unsigned short speed = (rpm > 12000) ? 12000 : rpm;
    int ret = bus->swrite(mb_dev, speed_addr, speed);
    if (0 == ret)
        motor_attr->rpm = rpm;

    return ret;
}

static int dmke_speed_get(xp_motor_t *motor, motor_rpm_t *rpm)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct dmke_motor_attr *motor_attr = (struct dmke_motor_attr *)(mb_dev->device_attr);

    unsigned short speed = 0;
    unsigned short speed_addr = 0x0006;
    int ret = bus->sread(mb_dev, speed_addr, &speed);
    *rpm = speed;

    return (ret == 0)?0:(-1);
}

static int dmke_direction_set(xp_motor_t *motor, motor_direction_t dir)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct dmke_motor_attr *motor_attr = (struct dmke_motor_attr *)(mb_dev->device_attr);

    unsigned short _dir = (dir == motor_forward) ? 0 : 1;
    unsigned dir_addr = 0x0023;
    int ret = 0;

    if (motor_attr->control_mode == 0) {
        xp_io_write_pin(1, motor_attr->io_dir, (_dir == 0) ? 1 : 0);  // motor_attr->io_dir 
    } else if (motor_attr->control_mode == 1) {
        ret = bus->swrite(mb_dev, dir_addr, _dir);
    } else ret = -1;
    if (0 == ret) motor_attr->dir = _dir;

    return ret;
}

static int dmke_direction_get(xp_motor_t *motor, motor_direction_t *dir)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct dmke_motor_attr *motor_attr = (struct dmke_motor_attr *)(mb_dev->device_attr);

    unsigned short _dir = 0;
    unsigned dir_addr = 0x0023;
    int ret = 0;

    if (motor_attr->control_mode == 0) {
        _dir = motor_attr->dir;
    } else if (motor_attr->control_mode == 1) {
        ret = bus->sread(mb_dev, dir_addr, &_dir);
    } else ret = -1;
    if (0 == ret) {
        motor_attr->dir = _dir;
        *dir = (_dir == 0) ? motor_forward : motor_reverser;
    }

    return ret;
}

static int dmke_rotate_by_angle(xp_motor_t *motor, int angle)
{
    return 0;
}

static int dmke_start(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct dmke_motor_attr *motor_attr = (struct dmke_motor_attr *)(mb_dev->device_attr);

    unsigned short run_addr = 0x0022;
    unsigned short start = 0x0001;
    int ret = 0;

    if (motor_attr->control_mode == 0) {
        xp_io_write_pin(1, motor_attr->io_ctrl, 0);     // motor_attr->io_ctrl
    } else if (motor_attr->control_mode == 1) {
        ret = bus->swrite(mb_dev, run_addr, start);
    } else ret = -1;

    return ret;
}

static int dmke_stop(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct dmke_motor_attr *motor_attr = (struct dmke_motor_attr *)(mb_dev->device_attr);

    unsigned short run_addr = 0x0022;
    unsigned short start = 0x0000;
    int ret = 0;

    if (motor_attr->control_mode == 0) {
        xp_io_write_pin(1, motor_attr->io_ctrl, 1);     // motor_attr->io_ctrl
    } else if (motor_attr->control_mode == 1) {
        ret = bus->swrite(mb_dev, run_addr, start);
    } else ret = -1;

    return ret;
}

static int dmke_emerg_enable(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct dmke_motor_attr *motor_attr = (struct dmke_motor_attr *)(mb_dev->device_attr);

    unsigned short emerg = 0x0001;
    unsigned short emerg_addr = 0x0024;
    int ret = 0;

    if (motor_attr->control_mode == 0) {
        xp_io_write_pin(1, motor_attr->io_emerg, 0);     // motor_attr->io_emerg
    } else if (motor_attr->control_mode == 1) {
        ret = bus->swrite(mb_dev, emerg_addr, emerg);
    } else {
        ret = -1;
    }

    return ret;
}

static int dmke_emerg_disable(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;
    struct dmke_motor_attr *motor_attr = (struct dmke_motor_attr *)(mb_dev->device_attr);

    unsigned short emerg = 0x0000;
    unsigned short emerg_addr = 0x0024;
    int ret = 0;

    if (motor_attr->control_mode == 0) {
        xp_io_write_pin(1, motor_attr->io_emerg, 1);     // motor_attr->io_emerg
    } else if (motor_attr->control_mode == 1) {
        ret = bus->swrite(mb_dev, emerg_addr, emerg);
    } else {
        ret = -1;
    }

    return ret;
}

static int dmke_error_get(xp_motor_t *motor, motor_error_t *err_code)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;

    unsigned short error_addr = 0x0111;

    int ret = bus->sread(mb_dev, error_addr, err_code);

    return ret;
}

static int dmke_error_clear(xp_motor_t *motor)
{
    LOCAL_ASSERT(motor);
    struct xp_modbus_device *mb_dev = (struct xp_modbus_device *)motor->private_data;
    struct xp_modbus_bus *bus = mb_dev->bus;

    unsigned short error = 0x0000;
    unsigned short error_addr = 0x0111;

    int ret = bus->swrite(mb_dev, error_addr, error);

    return ret;
}

struct xp_motor_stdops dmke_stdops = {
    .init = dmke_init,
    .speed_set = dmke_speed_set,
    .speed_get = dmke_speed_get,
    .direction_set = dmke_direction_set,
    .direction_get = dmke_direction_get,
    .rotate_by_angle = dmke_rotate_by_angle,
    .rotation_start = dmke_start,
    .rotation_stop = dmke_stop,
    .emerg_enable = dmke_emerg_enable,
    .emerg_disable = dmke_emerg_disable,
    .error_get = dmke_error_get,
    .error_clear = dmke_error_clear,
};

int xp_dmke_motor_constructor(struct xp_motor *motor, struct xp_modbus_device *dev, unsigned char *name)
{
    if (motor == NULL || dev == NULL || name == NULL)
        return -1;
    motor->private_data = (void *)dev;
    strncpy((char *)motor->name, (char *)name, XP_MOTOR_NAME_LENGTH);
    motor->ops = &dmke_stdops;

    if (NULL == motor->ops->init || 0 != motor->ops->init(motor))
    {
        motor->inited = 0;
        return -1;
    }

    motor->inited = 1;
    return 0;
}

/*********** DEBUG TEST ************/
//#if (XP_CONFIG_AGV_WHEEL_DMKE_ENABLE == 1)
struct xp_motor dmke_debug_motor_1 = {0};
struct xp_motor dmke_debug_motor_2 = {0};

#include "../../washer/line-ac/device_manager.h"
int xp_dmke_motor_debug(char *type, char *fun, char *param)
{
    if (strcmp(type, "dmke-1") != 0 &&
        strcmp(type, "dmke-2") != 0)
        return 0;

    int ret = 0;
    static int debug_inited = 0;

    struct xp_modbus_device *dmke_debug_dev_1 = get_modbus_device_by_name((unsigned char *)"dmke_leftwheel");
    struct xp_modbus_device *dmke_debug_dev_2 = get_modbus_device_by_name((unsigned char *)"dmke_rightwheel");

    if (dmke_debug_dev_1 == NULL || dmke_debug_dev_2 == NULL)
        return 0;

    if (dmke_debug_dev_1->inited != xp_true || dmke_debug_dev_2->inited != xp_true)
        return 0;

    if (!debug_inited) {
        debug_inited = 1;
        ret = xp_dmke_motor_constructor(&dmke_debug_motor_1, dmke_debug_dev_1, (unsigned char *)"dmke_debug_1");
        ret |= xp_dmke_motor_constructor(&dmke_debug_motor_2, dmke_debug_dev_2, (unsigned char *)"dmke_debug_2");
        if (ret != 0)
            return 0;
    }
    motor_direction_t dir = motor_forward;
    int id = 0;

    char *p = strstr(type, "-");
    if (p == NULL)
        return 0;
    p++;
    id = atoi(p);

    struct xp_motor dmke_debug_motor = {0};

    if (id == 1)
        dmke_debug_motor = dmke_debug_motor_1;
    else if (id == 2)
        dmke_debug_motor = dmke_debug_motor_2;
    else
        return 0;

    if (strcmp(fun, "set_speed") == 0)
    {
        println("set motor rpm = %d", atoi(param));
        ret = dmke_debug_motor.ops->speed_set(&dmke_debug_motor, atoi(param));
    }
    else if (strcmp(fun, "reset") == 0)
    {
        debug_inited = 0;
    }
    else if (strcmp(fun, "set_dir") == 0)
    {
        if (atoi(param) == 1)
            dir = motor_reverser;
        println("set motor dir = %d", dir);
        ret = dmke_debug_motor.ops->direction_set(&dmke_debug_motor, dir);
    }
    else if (strcmp(fun, "start") == 0)
    {
        println("set motor start");
        ret = dmke_debug_motor.ops->rotation_start(&dmke_debug_motor);
    }
    else if (strcmp(fun, "stop") == 0)
    {
        println("set motor stop");
        ret = dmke_debug_motor.ops->rotation_stop(&dmke_debug_motor);
    }
    else if (strcmp(fun, "emerg") == 0)
    {
        println("set motor emerg");
        ret = dmke_debug_motor.ops->emerg_enable(&dmke_debug_motor);
    }
    else if (strcmp(fun, "emerg_release") == 0)
    {
        println("set motor emerg");
        ret = dmke_debug_motor.ops->emerg_disable(&dmke_debug_motor);
    }

    return 1;
}
